﻿#pragma once

#include "kernel/kernel.h"
#include "math/linear_math.h"

namespace CAGD
{

/**
 * @brief 包围盒算法.
 *
 */
template <class T> class Bounding
{
  public:
    template <class T2> using BPntT = TCoordN<3, T2, CoordsType::Pnt>;
    template <class T2> using BVecT = TCoordN<3, T2, CoordsType::Vec>;

    /**
     * @brief 计算有向包围盒.
     * @return
     * @param points
     * @see
     */
    static BBox OBB(const std::vector<BPntT<T>> &points)
    {
        int n = points.size();

        // 坐标分离为 3 个随机变量
        Vector X(n), Y(n), Z(n);
        for (int i = 0; i < n; i++)
        {
            X[i] = points[i].X();
            Y[i] = points[i].Y();
            Z[i] = points[i].Z();
        }

        // 计算期望点
        double E[] = {0, 0, 0};
        E[0] = X.Sum() / n;
        E[1] = Y.Sum() / n;
        E[2] = Z.Sum() / n;

        // 计算协方差矩阵
        Matrix B(3, 3);
        for (int i = 0; i < n; i++)
        {
            B(0, 0) += X[i] * X[i];
            B(0, 1) += X[i] * Y[i];
            B(0, 2) += X[i] * Z[i];

            B(1, 0) += Y[i] * X[i];
            B(1, 1) += Y[i] * Y[i];
            B(1, 2) += Y[i] * Z[i];

            B(2, 0) += Z[i] * X[i];
            B(2, 1) += Z[i] * Y[i];
            B(2, 2) += Z[i] * Z[i];
        }
        B = B / n;

        // 计算 Xi_i 和 Xi_j 的协方差
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                B(i, j) -= E[i] * E[j];

        // 特征分解
        LinearSolver solver;
        Matrix V = solver.PassJacobi(B, n * 10);

        // 比较对角元，进行排序
        int order[] = {0, 1, 2};
        for (int i = 0; i < 3; i++)
        {
            for (int j = 1; j < 3; j++)
            {
                // 比较相邻的元素，如果后者小于前者就交换
                if (B(j, j) > B(j - 1, j - 1))
                {
                    int tmp = order[j];
                    order[j] = order[j - 1];
                    order[j - 1] = tmp;
                }
            }
        }

        // 三个主方向，按照从大到小的顺序
        BVec x(V(0, order[0]), V(1, order[0]), V(2, order[0]));
        BVec y(V(0, order[1]), V(1, order[1]), V(2, order[1]));
        BVec z(V(0, order[2]), V(1, order[2]), V(2, order[2]));
        x.Normalize();
        y.Normalize();
        z.Normalize();

        // 计算在主方向上的投影长度，需要将期望平移到原点
        BVec Ev(E[0], E[1], E[2]);
        Matrix M(3, 2);
        for (int i = 0; i < n; i++)
        {
            BVec v = points[i] - Ev;
            double p[] = {x * v, y * v, z * v};
            for (int j = 0; j < 3; j++)
            {
                if (M(j, 0) > p[j])
                    M(j, 0) = p[j];
                if (M(j, 1) < p[j])
                    M(j, 1) = p[j];
            }
        }

        // 记录在 x,y,z 正反方向的 3 个向量，以及期望点 Ev
        // 0 负 1 正
        BBox box;
        box.center = Ev;
        box.x1 = x * M(0, 0);
        box.x2 = x * M(0, 1);
        box.y1 = y * M(1, 0);
        box.y2 = y * M(1, 1);
        box.z1 = z * M(2, 0);
        box.z2 = z * M(2, 1);

        return box;
    }

    /**
     * @brief 计算坐标对齐的包围盒.
     * @return
     * @param points
     * @see
     */
    static BBox AABB(const std::vector<BPntT<T>> &points)
    {
        BBox box;
        box.center = {0, 0, 0};

        float left = FLT_MAX;
        float right = -FLT_MAX;
        float bottom = FLT_MAX;
        float top = -FLT_MAX;
        float far = FLT_MAX;
        float near = -FLT_MAX;

        for (auto &p : points)
        {
            left = std::min(left, p.X());
            right = std::max(right, p.X());

            bottom = std::min(bottom, p.Y());
            top = std::max(top, p.Y());

            near = std::max(near, p.Z());
            far = std::min(far, p.Z());

            box.center += p;
        }

        box.center /= points.size();
        box.x1 = {left - box.center.X(), 0, 0};
        box.x2 = {right - box.center.X(), 0, 0};
        box.y1 = {0, bottom - box.center.Y(), 0};
        box.y2 = {0, top - box.center.Y(), 0};
        box.z1 = {0, 0, far - box.center.Z()};
        box.z2 = {0, 0, near - box.center.Z()};

        return box;
    }
};

} // namespace CAGD
